#include <iostream>
#include <random>
#include <dds/dds.h>
#include "EndZxzl.h" // IDL生成的C语言头文件
#include "../../../include/log_client/LogClient.h"

// DDS实体句柄
static dds_entity_t participant;
static dds_entity_t topic;
static dds_entity_t subscriber;
static dds_entity_t reader;

// 清理DDS资源
static void cleanup()
{
    if (reader != DDS_HANDLE_NIL)
        dds_delete(reader);
    if (subscriber != DDS_HANDLE_NIL)
        dds_delete(subscriber);
    if (topic != DDS_HANDLE_NIL)
        dds_delete(topic);
    if (participant != DDS_HANDLE_NIL)
        dds_delete(participant);
}

// 生成随机执行器命令数据（用于测试）
EndZxzl_ActuatorCommandOutput generate_random_actuator_command()
{
    static std::mt19937 rng(std::random_device{}());

    // 定义各参数的合理范围
    std::uniform_real_distribution<double> pos_dist(-30.0, 30.0);     // 舵面位置 (度)
    std::uniform_real_distribution<double> rate_dist(-200.0, 200.0);  // 舵面速率 (度/秒)
    std::uniform_real_distribution<double> throttle_dist(0.0, 100.0); // 油门位置 (%)
    std::uniform_real_distribution<double> effort_dist(0.0, 100.0);   // 执行器出力 (%)
    std::uniform_real_distribution<double> health_dist(0.0, 100.0);   // 健康状态 (%)
    std::uniform_real_distribution<double> sat_dist(0.0, 100.0);      // 饱和程度 (%)
    std::uniform_real_distribution<double> cross_dist(-10.0, 10.0);   // 交叉耦合 (度)
    std::uniform_real_distribution<double> hyst_dist(-5.0, 5.0);      // 迟滞补偿 (度)
    std::uniform_real_distribution<double> back_dist(-5.0, 5.0);      // 回差补偿 (度)
    std::uniform_real_distribution<double> pred_dist(-2.0, 2.0);      // 预测定位 (度)
    std::uniform_int_distribution<long> type_dist(0, 3);              // 返回类型

    EndZxzl_ActuatorCommandOutput cmd;

    // 舵面位置和速率
    cmd.fin1_position = pos_dist(rng);
    cmd.fin2_position = pos_dist(rng);
    cmd.fin3_position = pos_dist(rng);
    cmd.fin4_position = pos_dist(rng);
    cmd.throttle_position = throttle_dist(rng);

    cmd.fin1_rate = rate_dist(rng);
    cmd.fin2_rate = rate_dist(rng);
    cmd.fin3_rate = rate_dist(rng);
    cmd.fin4_rate = rate_dist(rng);
    cmd.throttle_rate = rate_dist(rng);

    // 执行器状态参数
    cmd.actuator_effort = effort_dist(rng);
    cmd.health_status = health_dist(rng);
    cmd.saturation_level = sat_dist(rng);
    cmd.cross_coupling = cross_dist(rng);
    cmd.hysteresis_compensation = hyst_dist(rng);
    cmd.backlash_compensation = back_dist(rng);
    cmd.predictive_positioning = pred_dist(rng);

    // 返回类型
    cmd.return_type = type_dist(rng);

    return cmd;
}

int main()
{
    //初始化日志
    LogClient::getInstance().init("mock_0701_output_zxzl","mock_0701_output_zxzl");

    dds_return_t rc;
    void *samples[1];       // 存储DDS样本的数组（最多1个样本）
    dds_sample_info_t info; // 样本元信息（如有效性、时间戳等）

    // 1. 初始化DDS参与者（DomainParticipant）
    participant = dds_create_participant(DDS_DOMAIN_DEFAULT, NULL, NULL);
    if (participant < 0)
    {
        std::cerr << "错误：创建参与者失败，原因：" << dds_strretcode(-participant) << std::endl;
        LogClient::getInstance().logError("创建参与者失败，原因：" + std::string(dds_strretcode(-participant)));
        cleanup();
        return EXIT_FAILURE;
    }

    // 2. 创建主题（Topic），名称需与发布者一致
    topic = dds_create_topic(
        participant,
        &EndZxzl_ActuatorCommandOutput_desc, // IDL生成的类型描述
        "end_zxzl",                          // 主题名称（必须与发布者一致）
        NULL, NULL);
    if (topic < 0)
    {
        std::cerr << "错误：创建主题失败，原因：" << dds_strretcode(-topic) << std::endl;
        LogClient::getInstance().logError("创建主题失败，原因：" + std::string(dds_strretcode(-topic)));
        cleanup();
        return EXIT_FAILURE;
    }

    // 3. 创建订阅者（Subscriber）
    subscriber = dds_create_subscriber(participant, NULL, NULL);
    if (subscriber < 0)
    {
        std::cerr << "错误：创建订阅者失败，原因：" << dds_strretcode(-subscriber) << std::endl;
        LogClient::getInstance().logError("创建订阅者失败，原因：" + std::string(dds_strretcode(-subscriber)));
        cleanup();
        return EXIT_FAILURE;
    }

    // 4. 创建数据读取器（DataReader）
    dds_qos_t *qos = dds_create_qos();
    dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE, DDS_SECS(10));
    dds_qset_history(qos, DDS_HISTORY_KEEP_LAST, 10);

    reader = dds_create_reader(subscriber, topic, qos, NULL);
    dds_delete_qos(qos);

    if (reader < 0)
    {
        std::cerr << "错误：创建读取器失败，原因：" << dds_strretcode(-reader) << std::endl;
        LogClient::getInstance().logError("创建读取器失败，原因：" + std::string(dds_strretcode(-reader)));
        cleanup();
        return EXIT_FAILURE;
    }

    std::cout << "DDS执行器命令订阅者已启动，等待接收数据..." << std::endl;
    std::cout << "主题: end_zxzl" << std::endl;
    std::cout << "按Ctrl+C终止程序" << std::endl;
    LogClient::getInstance().logInfo("DDS执行器命令订阅者已启动，等待接收数据...\n主题: end_zxzl\n按Ctrl+C终止程序");

    // 5. 循环读取数据
    int count = 1;
    while (true)
    {
        // 分配样本内存（需与IDL生成的结构体类型匹配）
        samples[0] = EndZxzl_ActuatorCommandOutput__alloc();
        if (samples[0] == NULL)
        {
            std::cerr << "错误：分配样本内存失败" << std::endl;
            LogClient::getInstance().logError("分配样本内存失败");
            cleanup();
            return EXIT_FAILURE;
        }

        // 从读取器中获取数据（最多1个样本）
        rc = dds_take(reader, samples, &info, 1, 1);
        if (rc < 0)
        {
            std::cerr << "错误：dds_take失败，原因：" << dds_strretcode(-rc) << std::endl;
            LogClient::getInstance().logError("dds_take失败，原因：" + std::string(dds_strretcode(-rc)));
            EndZxzl_ActuatorCommandOutput_free(samples[0], DDS_FREE_ALL);
            cleanup();
            return EXIT_FAILURE;
        }

        // 检查是否有有效数据
        if (rc > 0 && info.valid_data)
        {
            // 转换为目标结构体类型
            EndZxzl_ActuatorCommandOutput *data =
                (EndZxzl_ActuatorCommandOutput *)samples[0];

            // 使用cout打印数据
            count++;
            std::string log_info = "接收到执行器命令，第" + std::to_string(count) + "次\n";
            log_info += "源时间戳: " + std::to_string(info.source_timestamp) + "\n";
            log_info += "[舵面状态]\n";
            log_info += "  舵面1: 位置=" + std::to_string(data->fin1_position) + "°\n"
                      + "  速率=" + std::to_string(data->fin1_rate) + "°/s\n";
                      + "  舵面2: 位置=" + std::to_string(data->fin2_position) + "°\n"
                      + "  速率=" + std::to_string(data->fin2_rate) + "°/s\n";
            log_info += "  舵面3: 位置=" + std::to_string(data->fin3_position) + "°\n"
                      + "  速率=" + std::to_string(data->fin3_rate) + "°/s\n";
            log_info += "  舵面4: 位置=" + std::to_string(data->fin4_position) + "°\n"
                      + "  速率=" + std::to_string(data->fin4_rate) + "°/s\n";
            log_info += "  油门: 位置=" + std::to_string(data->throttle_position) + "%\n"
                      + "  速率=" + std::to_string(data->throttle_rate) + "%/s\n";
            log_info += "  执行器状态: 出力=" + std::to_string(data->actuator_effort) + "%"
                      + "  健康=" + std::to_string(data->health_status) + "%"
                      + "  饱和=" + std::to_string(data->saturation_level) + "%" + "\n";
            log_info += "  补偿参数: 交叉耦合=" + std::to_string(data->cross_coupling) + "°"
                      + "  迟滞补偿=" + std::to_string(data->hysteresis_compensation) + "°\n"
                      + "  回差补偿=" + std::to_string(data->backlash_compensation) + "°\n";
            log_info += "  补偿参数: 预测定位=" + std::to_string(data->predictive_positioning) + "°\n";

            std::cout << log_info << std::endl;
            LogClient::getInstance().logInfo(log_info);
            // std::cout << "\n===== 接收到执行器命令 ===== 第" << count << "次"
            //           << "源时间戳: " << info.source_timestamp << std::endl;
            
            // // 舵面状态分组显示
            // std::cout << "[舵面状态]" << std::endl;
            // std::cout << "  舵面1: 位置=" << data->fin1_position << "°"
            //           << "  速率=" << data->fin1_rate << "°/s" << std::endl;
            // std::cout << "  舵面2: 位置=" << data->fin2_position << "°"
            //           << "  速率=" << data->fin2_rate << "°/s" << std::endl;
            // std::cout << "  舵面3: 位置=" << data->fin3_position << "°"
            //           << "  速率=" << data->fin3_rate << "°/s" << std::endl;
            // std::cout << "  舵面4: 位置=" << data->fin4_position << "°"
            //           << "  速率=" << data->fin4_rate << "°/s" << std::endl;
            // std::cout << "  油门: 位置=" << data->throttle_position << "%"
            //           << "  速率=" << data->throttle_rate << "%/s" << std::endl;

            // // 执行器状态分组显示
            // std::cout << "[执行器状态]" << std::endl;
            // std::cout << "  出力=" << data->actuator_effort << "%"
            //           << "  健康=" << data->health_status << "%"
            //           << "  饱和=" << data->saturation_level << "%" << std::endl;

            // // 补偿参数分组显示
            // std::cout << "[补偿参数]" << std::endl;
            // std::cout << "  交叉耦合=" << data->cross_coupling << "°"
            //           << "  迟滞补偿=" << data->hysteresis_compensation << "°"
            //           << "  回差补偿=" << data->backlash_compensation << "°" << std::endl;
            // std::cout << "  预测定位=" << data->predictive_positioning << "°" << std::endl;

            // 返回类型
            const char *type_str[] = {"成功", "警告", "错误", "超时"};
            long type_idx = (data->return_type >= 0 && data->return_type < 4) ? data->return_type : 0;
            std::cout << "[返回类型] " << type_str[type_idx]
                      << " (" << data->return_type << ")" << std::endl;
        }

        // 释放样本内存
        EndZxzl_ActuatorCommandOutput_free(samples[0], DDS_FREE_ALL);

        // 休眠100ms避免CPU占用过高
        dds_sleepfor(DDS_MSECS(100));
    }

    // 清理资源（实际不会执行到，可通过Ctrl+C终止程序）
    cleanup();
    return EXIT_SUCCESS;
}